/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : sliding_window_optimizer.h
 *@brief   : sliding window optimizer
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/13
 *
 */
#ifndef SLIDING_WINDOW_OPTIMIZER_H_
#define SLIDING_WINDOW_OPTIMIZER_H_

#include <ceres/ceres.h>

#include "frame/frame.h"
#include "optimizer/pose_local_parameterization.h"
#include "optimizer/rotation_local_parameterization.h"

#include "optimizer/imu_factor.h"
#include "optimizer/imu_bias_factor.h"
#include "optimizer/wheel_factor.h"
#include "optimizer/reprojection_factor.h"
#include "optimizer/velocity_factor.h"
#include "optimizer/marginalization_factor.h"

namespace optimizer {

using namespace frame;

class SlidingWindowOptimizer {
public:
    SlidingWindowOptimizer(std::shared_ptr<Frame>& frame_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id);
    void Solve();
private:
    void SolveSlidingWindowNotFul();
    void SolveSlidingWindowFul();
    void GetVisualFactorData(std::vector<Vector3d>& world_points, std::map<uint64_t, std::vector<std::pair<uint32_t, Vector2d>>>& undistorted_point, std::vector<uint64_t>& feature_ids, std::vector<std::shared_ptr<Feature>>& features_ptr);
    std::shared_ptr<Frame> frame_ptr_;

    uint8_t sliding_window_size_; 
    const Eigen::Matrix3d intrinsic_matrix_; 
    const Eigen::Matrix4d extrinsic_matrix_;

    MarginalizationInfo *last_marginalization_info_ = nullptr;
    std::vector<double *> last_marginalization_parameter_blocks_;

    uint8_t camera_id_;
};

}
#endif